#include "v3_msgs/PathGenerateManager.h"

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

#include <iostream>
#include <string>


class TestGlobalPathPlanner {
 public:
  TestGlobalPathPlanner();
  ~TestGlobalPathPlanner() {}
//   void SetPlanMode(uint8_t& plan_mode) { __plan_Mode = plan_mode; }

  void ReqP2P();
  void ReqBoustrophedon();
  void ReqLoop();
  void ReqEdge();
  void ReqEdgeBoustrophedon();
  void ReqEdgeLoop();

 private:
  ros::NodeHandle __nh;
//   ros::Subscriber __rviz_pose_sub;
  ros::ServiceClient __all_cover_path_client;
  ros::ServiceClient __p2p_path_client;

  // 订阅来自Rviz的目标点
//   void RvizPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
//   vector<geometry_msgs::PoseStamped> __poly_poses;  // 存放多边形点

//   uint8_t __plan_Mode;
};

TestGlobalPathPlanner::TestGlobalPathPlanner() {
//   __poly_poses.clear();
  // __rviz_pose_sub = __nh.subscribe("/move_base_simple/goal", 1, &TestGlobalPathPlanner::RvizPoseCallback, this);
  __all_cover_path_client = __nh.serviceClient<v3_msgs::PathGenerateManager>("/cover_path_server");
  __p2p_path_client = __nh.serviceClient<v3_msgs::PathGenerateManager>("/p2p_path_server");
  ros::Duration(1.0).sleep();
}

// // 订阅来自Rviz的目标点
// void TestGlobalPathPlanner::RvizPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
//   std::cout << "[RvizPoseCallback] receive a pose[x=" + std::to_string(msg->pose.position.x) +
//                    ", y=" + std::to_string(msg->pose.position.y) + "]" << std::endl;
//   // 接收数据的个数
//   int sz = 0;
//   if (__plan_Mode == 0)
//     sz = 2;
//   else if (__plan_Mode == 1)
//     sz = 5;

//   // 缓存位姿数据
//   __poly_poses.emplace_back(*msg);

//   if (__poly_poses.size() < sz) {
//     std::cout << "[RvizPoseCallback] insert " +
//                      std::to_string(__poly_poses.size()) + " pose successfully!" << std::endl;
//   } else if (__poly_poses.size() == sz) {
//     std::cout << "[RvizPoseCallback] insert " +
//                      std::to_string(__poly_poses.size()) + " pose successfully!" << std::endl;
//     v3_msgs::PathGenerateManager global_path_data;
//     // 设置轮廓
//     for (auto i = 0; i < __poly_poses.size() - 1; i++) {
//       global_path_data.request.path_gen.polygon.poses.emplace_back(__poly_poses[i].pose);
//     }
//     // 设置路径类型
//     v3_msgs::PathGenerateType path_type;
//     path_type.type = v3_msgs::PathGenerateType::WRAPPER_METHOD;
//     global_path_data.request.path_gen.path_types.emplace_back(path_type);
//     // path_type.type = v3_msgs::PathGenerateType::EDGE_METHOD;
//     // all_cover_client_data.request.path_gen.path_types.emplace_back(path_type);
//     // 设置二级路径类型
//     // 设置任务起点
//     global_path_data.request.path_gen.start = __poly_poses.back().pose;
//     // 设置任务终点
//     global_path_data.request.path_gen.end = __poly_poses.front().pose;
//     // 设置路径危险区域

//     if (msg->header.frame_id == "map") {
//       if ((sz == 5 && __all_cover_path_client.call(global_path_data)) ||
//           (sz == 2 && __p2p_path_client.call(global_path_data))) {
//         std::cout << "[RvizPoseCallback] /global_path_gen Service call OK." << std::endl;
//         std::cout << "path size:" << global_path_data.response.paths.size() << std::endl;
//         std::cout << "edge path size:" << global_path_data.response.edge_paths.size() << std::endl;
//       } else {
//         std::cout << "[RvizPoseCallback] /global_path_gen Service call failed." << std::endl;
//       }
//     } else if (msg->header.frame_id == "base_link") {
//       if ((sz == 5 && __all_cover_path_client.call(global_path_data)) ||
//           (sz == 2 && __p2p_path_client.call(global_path_data))) {
//         std::cout << "[RvizPoseCallback] /global_path_gen Service call OK." << std::endl;
//         std::cout << "path size:" << global_path_data.response.paths.size() << std::endl;
//         std::cout << "edge path size:" << global_path_data.response.edge_paths.size() << std::endl;
//       } else {
//         std::cout << "[RvizPoseCallback] /global_path_gen Service call failed." << std::endl;
//       }
//     }

//     __poly_poses.clear();
//   } else {
//     std::cout << "__poly_poses out of ranges.please retry." << std::endl;
//     __poly_poses.clear();
//   }
// }

void TestGlobalPathPlanner::ReqP2P() {
  v3_msgs::PathGenerateManager p2p_data;

  // 设置路径类型
  v3_msgs::PathGenerateType path_type;
  path_type.type = v3_msgs::PathGenerateType::P2P_METHOD;
  p2p_data.request.path_gen.path_types.emplace_back(path_type);

  // 设置任务起点
  p2p_data.request.path_gen.start.position.x = 12.058462;
  p2p_data.request.path_gen.start.position.y = 7.662326;
  // 设置任务终点
  p2p_data.request.path_gen.end.position.x = 15.299856;
  p2p_data.request.path_gen.end.position.y = 3.015029;

  std::cout << "To call p2p" << std::endl;
  __p2p_path_client.call(p2p_data);
  std::cout << "Call p2p finish" << std::endl;
}

void TestGlobalPathPlanner::ReqBoustrophedon() {
  v3_msgs::PathGenerateManager global_path_data;

  // 设置路径类型
  v3_msgs::PathGenerateType path_type;
  path_type.type = v3_msgs::PathGenerateType::BOUSTROPHEDON_METHOD;
  global_path_data.request.path_gen.path_types.emplace_back(path_type);

  // 设置轮廓
  std::vector<double> xy_vec{
      15.836044, 7.132683, 20.763098, 5.904231,
      19.469872, 0.717383, 14.542818, 1.945835,
  };
  for (size_t i = 0; i + 1 < xy_vec.size(); i += 2) {
    geometry_msgs::Pose tmp_pose;
    tmp_pose.position.x = xy_vec.at(i);
    tmp_pose.position.y = xy_vec.at(i + 1);
    global_path_data.request.path_gen.polygon.poses.emplace_back(tmp_pose);
  }

  // 设置任务起点
  global_path_data.request.path_gen.start.position.x = 14.5428;
  global_path_data.request.path_gen.start.position.y = 1.94584;

  global_path_data.request.path_gen.offset = 0;

  std::cout << "To call boustrophedon" << std::endl;
  __all_cover_path_client.call(global_path_data);
  std::cout << "Call boustrophedon finish" << std::endl;
}

void TestGlobalPathPlanner::ReqLoop() {
  v3_msgs::PathGenerateManager global_path_data;

  // 设置路径类型
  v3_msgs::PathGenerateType path_type;
  path_type.type = v3_msgs::PathGenerateType::WRAPPER_METHOD;
  global_path_data.request.path_gen.path_types.emplace_back(path_type);

  // 设置轮廓
  std::vector<double> xy_vec{
      15.836044, 7.132683, 20.763098, 5.904231,
      19.469872, 0.717383, 14.542818, 1.945835,
  };
  for (size_t i = 0; i + 1 < xy_vec.size(); i += 2) {
    geometry_msgs::Pose tmp_pose;
    tmp_pose.position.x = xy_vec.at(i);
    tmp_pose.position.y = xy_vec.at(i + 1);
    global_path_data.request.path_gen.polygon.poses.emplace_back(tmp_pose);
  }

  // 设置任务起点
  global_path_data.request.path_gen.start.position.x = 14.5428;
  global_path_data.request.path_gen.start.position.y = 1.94584;

  global_path_data.request.path_gen.offset = 0;

  std::cout << "To call loop" << std::endl;
  __all_cover_path_client.call(global_path_data);
  std::cout << "Call loop finish" << std::endl;
}

void TestGlobalPathPlanner::ReqEdge() {
  v3_msgs::PathGenerateManager global_path_data;

  // 设置路径类型
  v3_msgs::PathGenerateType path_type;
  path_type.type = v3_msgs::PathGenerateType::EDGE_METHOD;
  global_path_data.request.path_gen.path_types.emplace_back(path_type);

  // 设置轮廓
  std::vector<double> xy_vec{
      15.836044, 7.132683, 20.763098, 5.904231,
      19.469872, 0.717383, 14.542818, 1.945835,
  };
  for (size_t i = 0; i + 1 < xy_vec.size(); i += 2) {
    geometry_msgs::Pose tmp_pose;
    tmp_pose.position.x = xy_vec.at(i);
    tmp_pose.position.y = xy_vec.at(i + 1);
    global_path_data.request.path_gen.polygon.poses.emplace_back(tmp_pose);
  }

  // 设置任务起点
  global_path_data.request.path_gen.start.position.x = 14.5428;
  global_path_data.request.path_gen.start.position.y = 1.94584;

  global_path_data.request.path_gen.offset = 0;

  std::cout << "To call edge" << std::endl;
  __all_cover_path_client.call(global_path_data);
  std::cout << "Call edge finish" << std::endl;
}

void TestGlobalPathPlanner::ReqEdgeBoustrophedon() {
  v3_msgs::PathGenerateManager global_path_data;

  // 设置路径类型
  v3_msgs::PathGenerateType path_type;
  path_type.type = v3_msgs::PathGenerateType::EDGE_METHOD;
  global_path_data.request.path_gen.path_types.emplace_back(path_type);
  path_type.type = v3_msgs::PathGenerateType::BOUSTROPHEDON_METHOD;
  global_path_data.request.path_gen.path_types.emplace_back(path_type);

  // 设置轮廓
  std::vector<double> xy_vec{
      15.836044, 7.132683, 20.763098, 5.904231,
      19.469872, 0.717383, 14.542818, 1.945835,
  };
  for (size_t i = 0; i + 1 < xy_vec.size(); i += 2) {
    geometry_msgs::Pose tmp_pose;
    tmp_pose.position.x = xy_vec.at(i);
    tmp_pose.position.y = xy_vec.at(i + 1);
    global_path_data.request.path_gen.polygon.poses.emplace_back(tmp_pose);
  }

  // 设置任务起点
  global_path_data.request.path_gen.start.position.x = 14.5428;
  global_path_data.request.path_gen.start.position.y = 1.94584;

  global_path_data.request.path_gen.offset = 0;

  std::cout << "To call edge boustrophedon" << std::endl;
  __all_cover_path_client.call(global_path_data);
  std::cout << "Call edge boustrophedon finish" << std::endl;
}

void TestGlobalPathPlanner::ReqEdgeLoop() {
  v3_msgs::PathGenerateManager global_path_data;

  // 设置路径类型
  v3_msgs::PathGenerateType path_type;
  path_type.type = v3_msgs::PathGenerateType::EDGE_METHOD;
  global_path_data.request.path_gen.path_types.emplace_back(path_type);
  path_type.type = v3_msgs::PathGenerateType::WRAPPER_METHOD;
  global_path_data.request.path_gen.path_types.emplace_back(path_type);

  // 设置轮廓
  std::vector<double> xy_vec{
      15.836044, 7.132683, 20.763098, 5.904231,
      19.469872, 0.717383, 14.542818, 1.945835,
  };
  for (size_t i = 0; i + 1 < xy_vec.size(); i += 2) {
    geometry_msgs::Pose tmp_pose;
    tmp_pose.position.x = xy_vec.at(i);
    tmp_pose.position.y = xy_vec.at(i + 1);
    global_path_data.request.path_gen.polygon.poses.emplace_back(tmp_pose);
  }

  // 设置任务起点
  global_path_data.request.path_gen.start.position.x = 14.5428;
  global_path_data.request.path_gen.start.position.y = 1.94584;

  global_path_data.request.path_gen.offset = 0;

  std::cout << "To call edge loop" << std::endl;
  __all_cover_path_client.call(global_path_data);
  std::cout << "Call edge loop finish" << std::endl;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "test_global_path_planner");
  std::cout << "To test_gpp" << std::endl;

  ros::NodeHandle nh;
  nh.setParam("test_gpp", true);
  std::cout << "Set test_gpp true" << std::endl;

  TestGlobalPathPlanner test_global_path_planner;
  // 0 ---> p2p, 1 ---> all cover path, 2 ----> implement
  // uint8_t plan_mode = 0;
  // test_global_path_planner.SetPlanMode(plan_mode);
  // ros::spin();

  int test_case = 2;
  switch (test_case) {
    case 1:
      test_global_path_planner.ReqP2P();
      break;
    case 2:
      test_global_path_planner.ReqBoustrophedon();
      break;
    case 3:
      test_global_path_planner.ReqLoop();
      break;
    case 4:
      test_global_path_planner.ReqEdge();
      break;
    case 5:
      test_global_path_planner.ReqEdgeBoustrophedon();
      break;
    case 6:
      test_global_path_planner.ReqEdgeLoop();
      break;

    default:
      break;
  }

  nh.setParam("test_gpp", false);
  std::cout << "Set test_gpp false" << std::endl;

  return 0;
}